# from ast import Del
# from itertools import count
# from this import d
import numpy as np
import cv2
import cv2 as cv
from matplotlib import pyplot as plt
import glob
import time
from datetime import datetime


# 计算交点函数
def cross_point(line1, line2):
    # 是否存在交点
    point_is_exist = False
    x = 0
    y = 0
    x1, y1, x2, y2, flag = line1[0]
    x3, y3, x4, y4, flag1 = line2[0]

    if (x2 - x1) == 0:
        k1 = None
    else:
        k1 = (y2 - y1) * 1.0 / (x2 - x1)  # 计算k1,由于点均为整数，需要进行浮点数转化
        b1 = y1 * 1.0 - x1 * k1 * 1.0  # 整型转浮点型是关键

    if (x4 - x3) == 0:  # L2直线斜率不存在操作
        k2 = None
        b2 = 0
    else:
        k2 = (y4 - y3) * 1.0 / (x4 - x3)  # 斜率存在操作
        b2 = y3 * 1.0 - x3 * k2 * 1.0

    if k1 is None:
        if not k2 is None:
            x = x1
            y = k2 * x1 + b2
            point_is_exist = True
    elif k2 is None:
        x = x3
        y = k1 * x3 + b1
    elif not k2 == k1:
        x = (b2 - b1) * 1.0 / (k1 - k2)
        y = k1 * x * 1.0 + b1 * 1.0
        point_is_exist = True
    if x < 0 or y < 0:
        point_is_exist = False

    return point_is_exist, (int(x), int(y))


# 判断横线还是竖线
def horizontal_vertical(lines):
    s_count = 0
    h_count = 0
    h_lines = []
    s_lines = []
    for count, line in enumerate(lines):
        x1, y1, x2, y2 = line[0]

        if (abs(y1 - y2) > 100 and abs(x1 - x2) > 50 and abs(x1 - x2) < 350):
            # print("是竖线")
            lines[count][0].append('s')
            s_lines.append([lines[count][0]])
            s_count += 1
            # list
            # ndarray
        elif ((abs(x1 - x2) > abs(y1 - y2)) and abs(y1 - y2) < 20):
            # print("是横线")
            lines[count][0].append('h')
            h_lines.append([lines[count][0]])
            h_count += 1
            # new_list =np.append(lines[count][0],0)
            # np.append(lines[count][0],'1')
        else:
            lines[count][0].append('w')
            # del (lines[count])
        # print(lines[count][0])
        court_exist = True
    if (s_count == 0 or h_count == 0):
        print("没有找到横线或竖线")
        court_exist = False
    return court_exist, h_lines, s_lines


# 清除多余的线
def clear_duplicate_lines(lines):
    count = 0
    # if lines[0][0][4] =='h':
    for line in lines:
        x1, y1, x2, y2, flag = lines[count][0]

        count1 = 0
        # for line1 in lines[1:]:
        while True:

            if (count + 2 > len(lines)):
                return

            T_x1, T_y1, T_x2, T_y2, flag1 = lines[count + 1][0]

            dis = 25
            # print(lines[count][0],lines[count+1][0])
            if ((abs(T_y1 - y1) < dis or abs(T_y2 - y2) < dis)):

                del (lines[count + 1])
            else:
                count += 1
                break


def find_course(lines):
    min_y1, min_y2 = 999, 999

    min_x, max_x = 999, 0
    points = []
    left_line, Right_line, top_line, bottom_line = [], [], [], []

    for count, line in enumerate(lines):
        x1, y1, x2, y2, flag = line[0]
        # 找出最左和最右的竖线
        if line[0][4] == "s":
            _s, temp = cross_point(line, lines[0])
            # if(_s==True):
            # if line[0][1]>50:
            # if(count>2):
            #     if(abs(points[count-1][0]-points[count][0])<3):

            if (temp[0] < min_x):
                # if abs(temp[0]-min_x<3):
                left_line = line[0]
                min_x = temp[0]

            if (temp[0] > max_x):
                # if abs(temp[0]-max_x<3):
                Right_line = line[0]
                max_x = temp[0]

            # cv2.circle(image_copy,temp,3,(255,0,0),thickness=5)
            points.append(temp)
        # 找出顶线
        if flag == 'h':
            if (y1 < min_y1 and y1 < min_y2) and abs(y1 - lines[0][0][1]) > 50 and abs(y1 - lines[0][0][1]) < 150:
                min_y1, min_y2 = y1, y2
                top_line = line[0]

    top_line = lines[1][0]
    bottom_line = lines[0][0]

    top_line[1] -= 10
    top_line[3] -= 10

    # print(left_line,Right_line)
    # print(min_x1,max_x1)
    return left_line, Right_line, top_line, bottom_line


def draw_course(img, left_line, Right_line, top_line, bottom_line):
    if len(left_line) > 1:
        x1, y1, x2, y2, flag = left_line

        # x_q=(bottom_line[1]-y1)*(x2-x1)/(y2-y1)+x1
        _s, G_l = cross_point([left_line], [top_line])
        _s, G_r = cross_point([left_line], [bottom_line])
        cv2.line(img, G_l, G_r, (255, 0, 0), thickness=2)

        # cv2.line(img, (int(x_q), bottom_line[1]), (x2,y2), (255, 0, 0), thickness=2)
    if len(Right_line) > 1:
        x1, y1, x2, y2, flag = Right_line

        # x_r=(bottom_line[1]-y1)*(x2-x1)/(y2-y1)+x1

        _s, G_l = cross_point([Right_line], [top_line])
        _s, G_r = cross_point([Right_line], [bottom_line])

        # if(x_r<800):
        cv2.line(img, G_l, G_r, (255, 0, 0), thickness=2)

    if len(top_line) > 1:
        # top_line[1]-=5
        # top_line[3]-=5
        x1, y1, x2, y2, flag = top_line

        _s, G_l = cross_point([top_line], [left_line])
        _s, G_r = cross_point([top_line], [Right_line])

        cv2.line(img, G_l, G_r, (255, 0, 0), thickness=2)

    if len(bottom_line) > 1:
        x1, y1, x2, y2, flag = bottom_line

        _s, G_l = cross_point([bottom_line], [left_line])
        _s, G_r = cross_point([bottom_line], [Right_line])

        cv2.line(img, G_l, G_r, (255, 0, 0), thickness=2)
    return img


def sort(lines):
    # 从大到小排序（按照x1）
    for i, line1 in enumerate(lines):
        for j, line2 in enumerate(lines):
            # print(line1,line2)
            # print(line1[0][4])
            if line1[0][4] == 'h' and line2[0][4] == 'h':
                if lines[i][0][1] > lines[j][0][1]:  # 如果line1比line2大就换位
                    # if  line1[0][1]>line2[0][1]:#如果line1比line2大就换位
                    temp = lines[j][0]
                    lines[j][0] = lines[i][0]
                    lines[i][0] = temp


# def Get_Lines(img):
#     global edges,image_gray
#     # img= cv2.resize(img,(800,600))
#     # img= cv2.resize(img,(1440,1080))
#     img= cv2.resize(img,(1920,1080))
#
#     # 转换成灰度图
#     image_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
#     blurred = cv2.GaussianBlur(image_gray, (7, 3), 0)
#     # 边缘检测, Sobel算子大小为3
#     edges = cv2.Canny(blurred, 0, 255, apertureSize=3)
#     # edges = cv2.Canny(image_gray, 35, 225, 3)
#
#     # 霍夫曼直线检测
#     lines = cv2.HoughLinesP(edges, 1, np.pi / 180, 100, minLineLength=150, maxLineGap=50)
#     # print(len(lines))
#     # print(lines)
#
#     # 遍历
#     # print(type(lines))
#     if(len(lines)):
#         lines=lines.tolist()
#
#     return lines

def Get_Lines(img, filter_upper_half=True, upper_ratio=0.3):
    global edges, image_gray
    # img= cv2.resize(img,(800,600))
    img= cv2.resize(img,(1280,720))
    # img= cv2.resize(img,(1440,1080))
    # img = cv2.resize(img, (1920, 1080))

    # 转换成灰度图
    image_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(image_gray, (7, 3), 0)
    # 边缘检测, Sobel算子大小为3
    edges = cv2.Canny(blurred, 0, 255, apertureSize=3)
    # edges = cv2.Canny(image_gray, 35, 225, 3)

    # 霍夫曼直线检测
    lines = cv2.HoughLinesP(edges, 1, np.pi / 180, 100, minLineLength=150, maxLineGap=50)

    # 遍历
    if (len(lines)):
        lines = lines.tolist()

        # 过滤上半部分的线条
        if filter_upper_half:
            height = img.shape[0]
            upper_threshold = int(height * upper_ratio)  # 上半部分阈值

            filtered_lines = []
            for line in lines:
                x1, y1, x2, y2 = line[0]
                # 检查线条的两个端点是否都在上半部分
                if y1 > upper_threshold and y2 > upper_threshold:
                    filtered_lines.append(line)

            lines = filtered_lines
            print(f"🔍 过滤前线条数: {len(lines) + len(filtered_lines)}")
            print(f"🔍 过滤后线条数: {len(lines)}")

    return lines


# 在初始化场地检测
def detect_court_from_frame(frame):
    """
    从帧检测羽毛球场地 - 基于test.py的成功逻辑
    """
    frame_copy = frame.copy()
    height, width = frame_copy.shape[:2]
    print(f"🔍 帧尺寸: {width}x{height}")

    # 获取线条
    lines = Get_Lines(frame_copy)
    if not lines:
        print("❌ 未检测到任何线条")
        return None, None, None, None,None

    print(f"✅ 检测到 {len(lines)} 条线条")

    # 分类线条
    court_exist, h_lines, s_lines = horizontal_vertical(lines)
    if not court_exist:
        print("❌ 未找到足够的横线或竖线")
        return None, None, None, None,None

    print(f"✅ 横线数量: {len(h_lines)}")
    print(f"✅ 竖线数量: {len(s_lines)}")

    # 清理重复线条
    clear_duplicate_lines(h_lines)

    # 合并线条
    lines = h_lines + s_lines

    # 寻找场地边界
    left_line, right_line, top_line, bottom_line = find_course(lines)

    if not left_line or not right_line or not top_line or not bottom_line:
        print("❌ 未找到完整的场地边界")
        return None
    """
    从帧检测羽毛球场地 - 基于test.py的成功逻辑
    """
    frame_copy = frame.copy()
    height, width = frame_copy.shape[:2]
    print(f"🔍 帧尺寸: {width}x{height}")

    # 获取线条
    lines = Get_Lines(frame_copy)
    if not lines:
        print("❌ 未检测到任何线条")
        return None

    print(f"✅ 检测到 {len(lines)} 条线条")

    # 分类线条
    court_exist, h_lines, s_lines = horizontal_vertical(lines)
    if not court_exist:
        print("❌ 未找到足够的横线或竖线")
        return None

    print(f"✅ 横线数量: {len(h_lines)}")
    print(f"✅ 竖线数量: {len(s_lines)}")

    # 清理重复线条
    clear_duplicate_lines(h_lines)

    # 合并线条
    lines = h_lines + s_lines

    # 寻找场地边界
    left_line, right_line, top_line, bottom_line = find_course(lines)

    if not left_line or not right_line or not top_line or not bottom_line:
        print("❌ 未找到完整的场地边界")
        return None

    print("✅ 成功找到场地边界")

    return {
        'original_image': frame,
        'lines': lines,
        'h_lines': h_lines,
        's_lines': s_lines,
        'court_lines': {
            'left': left_line,
            'right': right_line,
            'top': top_line,
            'bottom': bottom_line
        }
    }

# 人员坐标转换函数
def Person_Coords(person_x, person_y,result):
    """
    将人员在图像中的像素坐标转换为球场坐标系中的坐标
    """
    left_line = result["court_lines"]["left"]
    right_line = result["court_lines"]["right"]
    top_line = result["court_lines"]["top"]
    bottom_line = result["court_lines"]["bottom"]
    lines = result["lines"]
    if (len(left_line)):
        x1, y1, x2, y2, flag = left_line
        left_x = (person_y - y1) * (x2 - x1) / (y2 - y1) + x1
    if (len(right_line)):
        x1, y1, x2, y2, flag = right_line
        right_x = (person_y - y1) * (x2 - x1) / (y2 - y1) + x1

    # 算出当前人所在的边界两点
    print("算出当前人所在的边界两点",left_x,right_x)

    pixel_to_CM = 610 / abs(right_x - left_x)  # 一个像素点对应的CM 610/(左线和右线的x距离)
    # X相对坐标
    G_x = (person_x - left_x) * pixel_to_CM  # （人的x-左线的x）* 一个像素点对应的CM

    pixel_to_CM = (410 / abs(bottom_line[1] - top_line[1]))  # 一个像素点对应的CM 410/(底线X-顶线X)
    # Y相对坐标
    G_y = (((lines[0][0][1] + lines[0][0][3])) / 2 - person_y) * pixel_to_CM
    # 像素点间距
    print(f"原始坐标: G_x={G_x}, G_y={G_y}")

    G_x = int(round(G_x))
    G_y = int(round(G_y))

    G_x = max(0,min(65535,G_x))
    G_y = max(0,min(65535,G_y))

    print(f"整数坐标: G_x={G_x}, G_y={G_y}")
    return G_x , G_y

if __name__ == '__main__':
    # for img
    # count1=0
    count1 = 0
    # for pic in glob.glob(r"C:\Users\W\Desktop\badminton\600p"+'\*.jpg'):
    # 读取图片
    # image = cv2.imread(r"C:\Users\W\yolov5\data\images\1.jpeg")
    # image = cv2.imread(r"C:\Users\W\yolov5\data\images\qq.jpg")
    # image = cv2.imread(r"C:\Users\W\yolov5\data\images\qq.jpg")
    # image = cv2.imread(r"C:\Users\W\Desktop\badminton\positive\IMG_20220728_173153.jpg")
    # image = cv2.imread(r"C:\Users\W\Desktop\badminton\positive\IMG_20220728_173558_1.jpg")
    # image = cv2.imread(r"C:\Users\W\Desktop\456.png")
    image = cv2.imread("image/frame_000097_result.jpg")

    # image = cv2.imread(pic)
    image_copy = image.copy()
    image_copy1 = image.copy()

    lines = Get_Lines(image_copy)

    h_lines = []
    s_lines = []
    # 判断是竖线还是横线
    court_exist, h_lines, s_lines = horizontal_vertical(lines)
    # 从大到小排序线
    sort(h_lines)
    # sort(lines)
    # print(lines)
    # 清除多余的线
    lines_w = h_lines + s_lines

    for count, line in enumerate(lines_w):
        x1, y1, x2, y2, flag = line[0]

        if line[0][4] == 'h':
            cv2.line(image_copy1, (0, y1), (1024, y2), (0, 0, 255), thickness=2)
        if line[0][4] == 's':
            cv2.line(image_copy1, (x1, y1), (x2, y2), (0, 255, 0), thickness=2)
    clear_duplicate_lines(h_lines)
    # print(lines)

    lines = h_lines + s_lines

    left_line = []
    right_line = []
    top_line = []
    bottom_line = []
    points = []
    print(lines)
    # 寻找球场线
    left_line, right_line, top_line, bottom_line = find_course(lines)

    # ------------------------------将人在图片中的位置转为球场坐标----------------------------------------------
    person_x = 613
    person_y = 341

    x1, y1, x2, y2, flag = left_line

    left_x = (person_y - y1) * (x2 - x1) / (y2 - y1) + x1
    # print(left_x)

    x1, y1, x2, y2, flag = right_line

    Right_x = (person_y - y1) * (x2 - x1) / (y2 - y1) + x1
    # print(Right_x)

    pixel_to_CM = 610 / (Right_x - left_x)  # 一个像素点对应的CM 610/(左线和右线的x距离)

    # X相对坐标
    G_x = (person_x - left_x) * pixel_to_CM  # （人的x-左线的x）* 一个像素点对应的CM

    abs_dd = 132  # 底线x和顶线x的间距
    pixel_to_CM = (410 / abs_dd)  # 一个像素点对应的CM 410/(底线X-顶线X)
    # Y相对坐标
    G_y = (((lines[0][0][1] + lines[0][0][3])) / 2 - person_y) * pixel_to_CM
    # 像素点间距
    print(G_x, G_y)
    # ------------------------------------------------------------------------------------------------

    # 画球场线
    for count, line in enumerate(lines):
        x1, y1, x2, y2, flag = line[0]

        if line[0][4] == 'h':
            cv2.line(image_copy, (0, y1), (1024, y2), (0, 0, 255), thickness=2)
        if line[0][4] == 's':
            cv2.line(image_copy, (x1, y1), (x2, y2), (0, 255, 0), thickness=2)

    # if len(top_lines)>1:
    #     x1, y1, x2, y2,flag = top_lines
    #     cv2.line(image_copy, (0, y1), (1024, y2), (255, 0, 0), thickness=2)
    # 画交点---------------------------------------------------------------------
    # if len(left_line)>1:
    #     # print(line[0])
    #     if x1!=0:
    #         _s,center=cross_point([left_line],line)
    #         # print(_s,center)
    #         if _s == True:
    #             cv2.circle(image_copy,center,3,(255,0,0),thickness=5)
    #             point.append(center)
    #  # print(point)
    # if len(Right_line)>1:
    #         # print(line[0])
    #         if y1!=0:
    #             _s,center=cross_point([Right_line],line)
    #             # print(_s,center)
    #             if _s == True:
    #                 cv2.circle(image_copy,center,3,(255,0,0),thickness=3)
    # ----------------------------------------------------------------------------

    if len(left_line) > 1:
        x1, y1, x2, y2, flag = left_line

        x_q = (bottom_line[1] - y1) * (x2 - x1) / (y2 - y1) + x1

        cv2.line(image_copy, (int(x_q), bottom_line[1]), (x2, y2), (255, 0, 0), thickness=2)
    if len(right_line) > 1:
        x1, y1, x2, y2, flag = right_line

        x_r = (bottom_line[1] - y1) * (x2 - x1) / (y2 - y1) + x1
        # if(x_r<800):
        cv2.line(image_copy, (x1, y1), (int(x_r), bottom_line[1]), (255, 0, 0), thickness=2)

    x1, y1, x2, y2, flag = top_line

    cv2.line(image_copy, (0, int(y1)), (800, int(y2)), (255, 0, 0), thickness=2)

    x1, y1, x2, y2, flag = bottom_line

    cv2.line(image_copy, (0, int(y1)), (800, int(y2)), (255, 0, 0), thickness=2)

    # x1, y1, x2, y2,flag = top_line
    # cv2.line(image_copy, (0, y1), (1024, y2), (0, 0, 255), thickness=2)

    i = str(count1)
    count1 += 1
    addr = r"C:\Users\W\Desktop\badminton\output2" + "\\" + i + ".jpg"
    addr1 = r"C:\Users\W\Desktop\badminton\output1" + "\\" + i + ".jpg"
    cv2.imwrite(addr, image_copy)
    # cv2.imwrite(addr1,edges)

    print("done", count1)
    # 图片展示
    f, ax = plt.subplots(2, 2, figsize=(12, 12))
    # cv2.imwrite(r"C:\Users\W\yolov5\output\123.jpg",image_copy)
    # 子图
    ax[0, 0].imshow(cv2.cvtColor(image, cv2.COLOR_BGR2RGB))
    ax[0, 1].imshow(cv2.cvtColor(edges, cv2.COLOR_BGR2RGB))
    ax[1, 0].imshow(cv2.cvtColor(image_copy1, cv2.COLOR_BGR2RGB))
    ax[1, 1].imshow(cv2.cvtColor(image_copy, cv2.COLOR_BGR2RGB))

    # 标题
    ax[0, 0].set_title("original")
    ax[0, 1].set_title("image edge")
    ax[1, 0].set_title("Before line cleaning")
    ax[1, 1].set_title("After line cleaning")

    plt.show()

